﻿using NetTopologySuite.Mathematics;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Math = System.Math;

namespace AgCIM.Tools.SpatialEncoding.Beidou.Common
{
    public class GisUtilDouble
    {

        public static readonly Vector3D wgs84OneOverRadii = new Vector3D(1.0 / 6378137.0, 1.0 / 6378137.0, 1.0 / 6356752.3142451793);
        public static readonly Vector3D wgs84OneOverRadiiSquared = new Vector3D(1.0 / (6378137.0 * 6378137.0), 1.0 / (6378137.0 * 6378137.0), 1.0 / (6356752.3142451793 * 6356752.3142451793));
        /// <summary>
        /// Cesium笛卡尔坐标转经纬度
        /// </summary>
        /// <param name="cartesian"></param>
        /// <returns></returns>
        public static Vector3D CesiumFromCartesian(Vector3D cartesian)
        {
            Vector3D oneOverRadii = wgs84OneOverRadii;
            Vector3D oneOverRadiiSquared = wgs84OneOverRadiiSquared;
            double centerToleranceSquared = 0.1;
            //`cartesian is required.` is thrown from scaleToGeodeticSurface
            Vector3D p = scaleToGeodeticSurface(cartesian, oneOverRadii, oneOverRadiiSquared, centerToleranceSquared);
            if (p == null)
            {
                return null;
            }
            Vector3D n = p * oneOverRadiiSquared;
            n = n.Normalize();
            Vector3D h = cartesian - p;
            double longitude = Math.Atan2(n.Y, n.X);
            double latitude = Math.Asin(n.Z);
            double height = Sign(h.Dot(cartesian)) * h.Length();
            return new Vector3D(longitude, latitude, height);
        }
        public static Vector3D scaleToGeodeticSurface(Vector3D cartesian, Vector3D oneOverRadii, Vector3D oneOverRadiiSquared, double centerToleranceSquared)
        {
            double positionX = cartesian.X;
            double positionY = cartesian.Y;
            double positionZ = cartesian.Z;

            double oneOverRadiiX = oneOverRadii.X;
            double oneOverRadiiY = oneOverRadii.Y;
            double oneOverRadiiZ = oneOverRadii.Z;

            double x2 = positionX * positionX * oneOverRadiiX * oneOverRadiiX;
            double y2 = positionY * positionY * oneOverRadiiY * oneOverRadiiY;
            double z2 = positionZ * positionZ * oneOverRadiiZ * oneOverRadiiZ;

            // Compute the squared ellipsoid norm.
            double squaredNorm = x2 + y2 + z2;
            double ratio = Math.Sqrt(1.0 / squaredNorm);

            // As an initial approximation, assume that the radial intersection is the projection point.
            Vector3D intersection = cartesian * ratio;

            // If the position is near the center, the iteration will not converge.
            if (squaredNorm < centerToleranceSquared)
            {
                if (double.IsInfinity(ratio))
                    return null;
            }

            double oneOverRadiiSquaredX = oneOverRadiiSquared.X;
            double oneOverRadiiSquaredY = oneOverRadiiSquared.Y;
            double oneOverRadiiSquaredZ = oneOverRadiiSquared.Z;

            // Use the gradient at the intersection point in place of the true unit normal.
            // The difference in magnitude will be absorbed in the multiplier.            
            double x = intersection.X * oneOverRadiiSquaredX * 2.0;
            double y = intersection.Y * oneOverRadiiSquaredY * 2.0;
            double z = intersection.Z * oneOverRadiiSquaredZ * 2.0;
            Vector3D gradient = new Vector3D(x, y, z);

            // Compute the initial guess at the normal vector multiplier, lambda.
            double lambda = (1.0 - ratio) * cartesian.Length() / (0.5 * gradient.Length());
            double correction = 0.0;

            double func;
            double denominator;
            double xMultiplier;
            double yMultiplier;
            double zMultiplier;
            double xMultiplier2;
            double yMultiplier2;
            double zMultiplier2;
            double xMultiplier3;
            double yMultiplier3;
            double zMultiplier3;

            do
            {
                lambda -= correction;

                xMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredX);
                yMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredY);
                zMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredZ);

                xMultiplier2 = xMultiplier * xMultiplier;
                yMultiplier2 = yMultiplier * yMultiplier;
                zMultiplier2 = zMultiplier * zMultiplier;

                xMultiplier3 = xMultiplier2 * xMultiplier;
                yMultiplier3 = yMultiplier2 * yMultiplier;
                zMultiplier3 = zMultiplier2 * zMultiplier;

                func = x2 * xMultiplier2 + y2 * yMultiplier2 + z2 * zMultiplier2 - 1.0;

                // "denominator" here refers to the use of this expression in the velocity and acceleration
                // computations in the sections to follow.
                denominator = x2 * xMultiplier3 * oneOverRadiiSquaredX + y2 * yMultiplier3 * oneOverRadiiSquaredY + z2 * zMultiplier3 * oneOverRadiiSquaredZ;
                double derivative = -2.0 * denominator;
                correction = func / derivative;
            } while (Math.Abs(func) > 1e-12);

            return new Vector3D(positionX * xMultiplier, positionY * yMultiplier, positionZ * zMultiplier);
        }

        public static int Sign(double value)
        {
            if (value == 0)
                return 0;
            return value > 0 ? 1 : -1;
        }
    }
}
